#include "battery/rebot_battery.hpp"
#include <socketcan_interface/string.h>
#include <socketcan_interface/socketcan.h>
#include <cmath>

RebotBattery::RebotBattery() 
  : Node("battery_node")
{
  if (!init()) {
    RCLCPP_FATAL(get_logger(), "Battery node initialization failed");
    rclcpp::shutdown();
  }
}

RebotBattery::~RebotBattery() 
{
  if (driver_) driver_->shutdown();
}

bool RebotBattery::init()
{
  // 参数声明与获取
  declare_parameter<int>("low_limit", 20);
  declare_parameter<int>("too_low_limit", 5);
  
  low_limit_ = get_parameter("low_limit").as_int();
  too_low_limit_ = get_parameter("too_low_limit").as_int();
  
  RCLCPP_INFO(get_logger(), "Low limit: %d, Too low limit: %d", 
              low_limit_, too_low_limit_);
  
  if (too_low_limit_ >= low_limit_) {
    RCLCPP_ERROR(get_logger(), "too_low_limit must be less than low_limit!");
    too_low_limit_ = low_limit_ - 1;
  }

  // 初始化事件管理器
  event_manager_.init(shared_from_this());

  // CAN设备初始化
  std::string can_device = declare_parameter<std::string>("can_device", "can1");
  driver_ = std::make_shared<can::ThreadedSocketCANInterface>();
  
  if (!driver_->init(can_device, 0, can::NoSettings::create())) {
    RCLCPP_FATAL(get_logger(), "CAN device %s initialization failed", can_device.c_str());
    return false;
  }
  
  frame_listener_ = driver_->createMsgListener(
    std::bind(&RebotBattery::battery_info_cb, this, std::placeholders::_1));

  // ROS2 订阅者/发布者
  battery_info_pub_ = create_publisher<agv_msgs::msg::BatInfo>("battery_info", 10);
  mode_sub_ = create_subscription<agv_msgs::msg::AGVMode>(
    "/agvmode", 10, 
    std::bind(&RebotBattery::mode_callback, this, std::placeholders::_1));

  // 定时器 (1Hz)
  timer_ = create_wall_timer(
    std::chrono::seconds(1),
    std::bind(&RebotBattery::timer_callback, this));
    
  return true;
}

void RebotBattery::mode_callback(const agv_msgs::msg::AGVMode::SharedPtr msg)
{
  agv_mode_ = *msg;
}

void RebotBattery::battery_info_cb(const can::Frame &msg)
{
  if (!msg.isValid()) {
    RCLCPP_ERROR(get_logger(), "Invalid CAN frame: ID %#04x, DLC %d", 
                 msg.id, msg.dlc);
    return;
  }

  if (msg.id == 0x115) {
    battery_info_.numeric_bat = msg.data[4] * 0.4f;
    battery_info_.voltage = (msg.data[6] << 8 | msg.data[7]) * 0.1f;
    RCLCPP_DEBUG(get_logger(), "SOC: %.1f%%, Voltage: %.1fV", 
                battery_info_.numeric_bat, battery_info_.voltage);
  }
  
  if (msg.id == 0x1A1) {
    battery_info_.charge_flag = (msg.data[4] == 0);
    float factor = battery_info_.charge_flag ? -0.001f : -0.1f;
    battery_info_.current = (msg.data[0] << 8 | msg.data[1]) * factor;
  }
  
  battery_info_pub_->publish(battery_info_);
}

void RebotBattery::battery_low_check()
{
  // 手动模式或充电中不触发事件
  if (agv_mode_.mode == agv_msgs::msg::AGVMode::MANUAL || 
      battery_info_.charge_flag) 
  {
    event_manager_.clearEvent();
    return;
  }

  if (battery_info_.numeric_bat < too_low_limit_) {
    event_manager_.updateEvent(
      agv_msgs::msg::AGVEvent::BATTERY_TOO_LOW_EVENT,
      agv_msgs::msg::AGVEvent::STOP,
      agv_msgs::msg::AGVEvent::ALARM,
      "Battery critically low! Emergency stop required",
      static_cast<int>(battery_info_.numeric_bat),
      too_low_limit_
    );
    
    if (event_manager_.checkEvent(agv_msgs::msg::AGVEvent::BATTERY_LOW_EVENT)) {
      event_manager_.removeEvent(agv_msgs::msg::AGVEvent::BATTERY_LOW_EVENT);
    }
  } 
  else if (battery_info_.numeric_bat < low_limit_) {
    event_manager_.updateEvent(
      agv_msgs::msg::AGVEvent::BATTERY_LOW_EVENT,
      agv_msgs::msg::AGVEvent::NONE,
      agv_msgs::msg::AGVEvent::ALARM,
      "Battery low warning",
      static_cast<int>(battery_info_.numeric_bat),
      low_limit_
    );
    
    if (event_manager_.checkEvent(agv_msgs::msg::AGVEvent::BATTERY_TOO_LOW_EVENT)) {
      event_manager_.removeEvent(agv_msgs::msg::AGVEvent::BATTERY_TOO_LOW_EVENT);
    }
  } 
  else {
    event_manager_.clearEvent();
  }
}

void RebotBattery::timer_callback()
{
  battery_low_check();
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RebotBattery>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}